

#include <stdarg.h>
#include <stdint.h>
#include <string.h>
#include <stddef.h>

#include "compiler.h"
#include "hcb_comn.h"



/* uart */
#define UART_BASE               BASE_AP_UART0

/* System Clock */
#define PCLKFREQ                (100 * 1000000)
#define BAUD_RATE(n)		    (PCLKFREQ / (n) / 16)



int  uart_earyly_init( void )
{
	uint32_t  temp;

	/* Set DLAB to 1 */
	write_reg( UART_BASE + UARTC_LCR_OFFSET, 0x83 );
  
	/* Set DLL for baudrate */
	write_reg( UART_BASE + UARTC_DLL_OFFSET, BAUD_RATE(115200) & 0xff );
	write_reg( UART_BASE + UARTC_DLH_OFFSET, (BAUD_RATE(115200) >> 8) & 0xff );

	/* baud 38400 , LCR: Length 8, no parity, 1 stop bit. */
	write_reg( UART_BASE + UARTC_LCR_OFFSET, 0x03 );

	/* IER to */
	write_reg( UART_BASE + UARTC_IER_OFFSET, 0x80 );
  	
	/* FCR: Enable FIFO, reset TX and RX. */
	write_reg( UART_BASE + UARTC_FCR_OFFSET, 0x97 );
	return 0;
}


static void  dbg_uart_puts( const char * ptr, int tlen )
{
    int  i;
	uint32_t  status;
	
	/**/
	for ( i=0; i<tlen; i++ )
	{
		status = read_reg( UART_BASE + UARTC_LSR_OFFSET );
		while ( (status & 0x20 ) == 0x20 )
		{
			status = read_reg( UART_BASE + UARTC_LSR_OFFSET );
		}
		
		/**/
		write_reg( UART_BASE + UARTC_THR_OFFSET, (uint32_t)ptr[i] );
	}
	
	return;
}


static int  mini_itoa(int value, unsigned int radix, unsigned int uppercase, unsigned int unsig, char *buffer, int zero_pad )
{
	char * pbuffer = buffer;
	int	 negative = 0;
	int	 i, len;

	/* No support for unusual radixes. */
	if (radix > 16)
		return 0;

	if (value < 0 && !unsig) {
		negative = 1;
		value = -value;
	}

	/* This builds the string back to front ... */
	do {
		int digit = value % radix;
		*(pbuffer++) = (digit < 10 ? '0' + digit : (uppercase ? 'A' : 'a') + digit - 10);
		value /= radix;
	} while (value > 0);

	for (i = (pbuffer - buffer); i < zero_pad; i++)
		*(pbuffer++) = '0';

	if (negative)
		*(pbuffer++) = '-';

	*(pbuffer) = '\0';

	/* ... now we reverse it (could do it recursively but will
	 * conserve the stack space) */
	len = (pbuffer - buffer);
	for (i = 0; i < len / 2; i++) {
		char j = buffer[i];
		buffer[i] = buffer[len-i-1];
		buffer[len-i-1] = j;
	}

	return len;
}


int  mini_vprintf( const char * fmt, va_list vst )
{
    char  ch;
	int  tcnt = 0;
    char  tch;
    const char * ptr;
    int  ccnt;
    int  zero_pad;
    char  bf[28];

    while( 1 )
    {
        ch = *(fmt++);
        if ( ch == '\0' )
        {
            break;
        }

        /**/        
        if ( ch != '%' )
        {
            ptr = fmt - 1;
            ccnt = 1;
            tcnt += 1;

            /**/
            while ( 1 )
            {
                ch = *(fmt++);
                if ( ch == '\0' )
                {
                    /* flush */
                    dbg_uart_puts( ptr, ccnt );
                    goto  _final_rtn;
                }

                if ( ch == '%' )
                {
                    /* flush */
                    dbg_uart_puts( ptr, ccnt );
                    goto  _fmt_swi;
                }

                /**/
                ccnt += 1;
                tcnt += 1;
            }
        }
        else
        {
_fmt_swi:
            ptr = bf;
            zero_pad = 0;
            ch = *(fmt++);
            if ( ch == '\0' )
            {
                /* nothing */
                goto  _final_rtn;
            }

            if ( ch == '0' )
            {
                ch = *(fmt++);
                if ( ch == '\0' )
                {
                    /* nothing */
                    goto  _final_rtn;
                }

                if (ch >= '0' && ch <= '9')
                {
					zero_pad = ch - '0';

                    ch = *(fmt++);
                    if ( ch == '\0' )
                    {
                        /* nothing */
                        goto  _final_rtn;
                    }
                }
            }

            /**/
            switch ( ch )
            {
            case 'p':
            case 'x':
                ccnt = mini_itoa( va_arg(vst, unsigned int), 16, 0, 1, bf, zero_pad );
                break;

            case 'X':
                ccnt = mini_itoa( va_arg(vst, unsigned int), 16, 1, 1, bf, zero_pad );
                break;
            
            case 'd':
                ccnt = mini_itoa( va_arg(vst, int), 10, 0, 0, bf, zero_pad );
                break;

            case 'u':
                ccnt = mini_itoa( va_arg(vst, unsigned int), 10, 0, 1, bf, zero_pad );
                break;

            case 'c':
                bf[0] = va_arg( vst, int );
                ccnt = 1;
                break;

            case 's':
                ptr = va_arg( vst, char* );
                ccnt = strlen( ptr );
                break;

            default:
                bf[0] = ch;
                ccnt = 1;
            }

            /**/
            dbg_uart_puts( ptr, ccnt );
            tcnt += ccnt;
        }

    }

_final_rtn:
    return tcnt;
}


int  printf( const char * fmt, ... )
{
    int  iret;
    va_list  vst;

    /**/
	va_start(vst, fmt);
    iret = mini_vprintf( fmt, vst );
    va_end(vst);

    /**/
    return iret;
}


void _panic( void *caller, const char *fmt, ... )
{
    int  iret;
    va_list  vst;

    /**/
    printf( "Panic!! caller: %p\n", caller );

    /**/
	va_start(vst, fmt);
    mini_vprintf( fmt, vst );
    va_end(vst);

    /**/
    while(1) {
        asm volatile ( "nop" );
    }
}


static  int  sidx;
static  char  spad[20];


void  dbg_excp_cli( void )
{
    int  i;
    uint32_t  status;
    uint8_t  tch;
    uint32_t * ptr;

    while ( 1 )
    {
        /* read line */
        while (1)
        {
            status = read_reg( UART_BASE + UARTC_LSR_OFFSET);
            if ( 0 != (status & SERIAL_LSR_DRDY) )
            {
                break;
            }
        }

        /**/
        tch = (uint8_t)read_reg( UART_BASE + UARTC_RBR_OFFSET);

        /* interpreter */
        if ((tch < 'a') || (tch > 'z'))
        {
            printf( "%08x\n", tch );
            continue;
        }

   
        if ( tch == 'b' )
        {
            for ( i=0; i<64; i++ )
            {
                printf( "%p: %08x %08x %08x %08x\n", ptr, ptr[0], ptr[1], ptr[2], ptr[3] );
                ptr += 4;
            }
        }

    }

}




struct excp_stk_frame {
    uintptr_t   r13;
    uintptr_t   r14;
    uintptr_t   excp;
    uintptr_t   cpsr;

    uintptr_t   r0;
    uintptr_t   r1;
    uintptr_t   r2;
    uintptr_t   r3;

    uintptr_t   r4;
    uintptr_t   r5;
    uintptr_t   r6;
    uintptr_t   r7;

    uintptr_t   r8;
    uintptr_t   r9;
    uintptr_t   r10;
    uintptr_t   r11;

    uintptr_t   r12;
    uintptr_t   r15;

};


void  dbg_exception_process( int excp, struct excp_stk_frame * pfrm )
{
    uint32_t  temp;

    /**/
    printf( "\nException=%02x, psr=%08x\n", excp, pfrm->cpsr );
    printf( " r0=%08x,  r1=%08x,  r2=%08x,  r3=%08x\n", pfrm->r0, pfrm->r1, pfrm->r2, pfrm->r3 );
    printf( " r4=%08x,  r5=%08x,  r6=%08x,  r7=%08x\n", pfrm->r4, pfrm->r5, pfrm->r6, pfrm->r7 );
    printf( " r8=%08x,  r9=%08x, r10=%08x, r11=%08x\n", pfrm->r8, pfrm->r9, pfrm->r10, pfrm->r11 );
    printf( "r12=%08x,  sp=%08x,  lr=%08x,  pc=%08x\n", pfrm->r12, pfrm->r13, pfrm->r14, pfrm->r15 );

    if ( excp == 0x10 )
    {
        temp = __builtin_arm_mrc( 15, 0, 5, 0, 0 );
        printf( "fault cause: %08x\n", temp );

        temp = __builtin_arm_mrc( 15, 0, 6, 0, 0 );
        printf( "fault adres: %08x\n", temp );
    }

    /**/
    dbg_excp_cli();
    return;
}


